#!/usr/bin/python2.4

# C6304: copyright missing
# pylint: disable-msg=C6304


"""Collection of useful graphics routines for coordinate transformation."""

import math
import StringIO
import sys


def Ratio(m_a, m_b, m_c, a_a, a_c):
  """Given a model ratio, compute the actual value.

  When m_a <= m_b < m_c, compute a_b in a_a <= a_b < a_c
  so that the position of m_b within m_a..m_c
  is the same as a_e within a_a..af_f

  Args:
    m_a: The model A value.
    m_b: The model B value.
    m_c: The model C value.
    a_a: The actual A value.
    a_c: The actual C value.

  Returns:
    a_b
  """
  if m_c == m_a:
    return a_c
  ratio = (1.0 * (m_b - m_a)) / (1.0 * (m_c - m_a))
  return a_a + ratio * (a_c - a_a)


class Polar(object):
  """A point represented in polar coordinates.

  The rho value is generally taken to be nm or knots.

  The theta value is ALWAYS in degrees, and is generally taken relative
  to true north, or the boat coordinate system.
  """

  def __init__(self, rho, theta):
    self._rho = rho
    self._theta = math.fmod(theta, 360.0)
    if self._theta < 0:
      self._theta += 360.0

  def Mag(self):
    return self._rho

  def Magnitude(self):
    return self._rho

  def Rho(self):
    return self._rho

  def Angle(self):
    return self._theta

  def Theta(self):
    return self._theta

  def __str__(self):
    return '(%05.2f %03.0f)' % (self._rho, self._theta)

  def __eq__(self, p1):
    return self._rho == p1._rho and self._theta == p1._theta

  def toRectangular(self):
    rads = math.radians(self._theta)
    # Remember, the coordinate system rotates CW with 0 degrees heads up
    return Point(
        self._rho * math.sin(rads),
        self._rho * math.cos(rads))

  def __add__(self, p1):
    r0 = self.toRectangular()
    r1 = p1.toRectangular()
    dx = r0.x + r1.x
    dy = r0.y + r1.y
    try:
      magnitude = math.hypot(dx, dy)
    except OverflowError, unused_e:
      magnitude = float('inf')
    angle = math.degrees(math.atan2(dx, dy))  # to compass coords
    return Polar(magnitude, angle)

  def __sub__(self, p1):
    r0 = self.toRectangular()
    r1 = p1.toRectangular()
    dx = r0.x - r1.x
    dy = r0.y - r1.y
    try:
      magnitude = math.hypot(dx, dy)
    except OverflowError, unused_e:
      magnitude = float('inf')
    angle = math.degrees(math.atan2(dx, dy))  # to compass coords
    return Polar(magnitude, angle)

  def CSVFormat(self):
    return '%05.2f,%03.0f' % (self._rho, self._theta)

  def ConstructorFormat(self):
    return 'Polar(%05.2f,%03.0f)' % (self._rho, self._theta)

  @staticmethod
  def BuildFromCSV(row, i):
    """Given a row and an index into the row, extract 2 columns as a Polar.

    This is the inverse of CSVFormat.
    Args:
      row: the row as read from the csv.
      i: which column number (0-based) to start extracting values.

    Returns:
      A new Polar, and the next[sic] position to start reading from.
    """
    polar = Polar(float(row[i+0]), float(row[i+1]))
    return polar, i+2-1  # the caller assumes the whole item is length 1


class Point(object):
  """A point represented in rectangular coordinates."""

  def __init__(self, x=0, y=0):
    self.x = x
    self.y = y

  def DistanceFrom(self, point):
    return math.hypot(self.x - point.x, self.y - point.y)

  def AngleFrom(self, point, xscale=1.0, yscale=1.0):
    # TODO(rrh): WATCHOUT: Compensate for lat/lon distortions(?).
    return math.degrees(math.atan2(
        xscale * (self.x - point.x),
        yscale * (self.y - point.y)))

  def KeyByAxis(self, axis):
    if axis == 0:
      return self.x
    elif axis == 1:
      return self.y
    else:
      raise Exception('bad axis')

  def SquareDistance(self, p2):
    dx = self.x - p2.x
    dy = self.y - p2.y
    return dx * dx + dy * dy

  def __str__(self):
    return '(%4g, %4g)' % (self.x, self.y)

  def __eq__(self, p1):
    return self.x == p1.x and self.y == p1.y

  def __hash__(self):
    as_str = self.__str__()
    h = hash(as_str)
    return h


def DM(degrees, minutes=0):
  """Convert degrees and minutes to minutes.

  Args:
    degrees: Degree value.
    minutes: Minutes value (should be between 0 and 59)

  Returns:
    The number of minutes represented by degrees and minutes.
  """
  return minutes + degrees * 60


class LatLon(object):
  """Models a latitude/longitude pair.

  WATCHOUT: latitude comes first in the constructor, per convention.
  latitude is equivalent to the y coordinate, and longitude to the x coordinate.
  """

  def __init__(self, lat, lon):
    """Constructor.

    Args:
      lat: latitude, in minutes.
      lon: longitude, in minutes.
    """
    self.lat = lat
    self.lon = lon

  def __str__(self):
    """Return a string that is parsimonious about using spaces."""
    return '(%4s, %4s)' % (
        LatMinutesAsString(self.lat), LonMinutesAsString(self.lon))

  def FullPrint(self):
    """Return a string with the full coordinates."""
    return '(%4sN, 12%4sW)' % (
        LatMinutesAsString(self.lat), LonMinutesAsString(self.lon))

  def DMFormat(self):
    """Return a string which when evaluated yields an identical value."""
    xlat = abs(self.lat)
    xlon = abs(self.lon)
    if self.lat < 0:
      slat = '-'
    else:
      slat = ' '
    if self.lon < 0:
      slon = '-'
    else:
      slon = ' '
    return 'LatLon(%sDM(%3d, %2d), %sDM(%3d, %2d))' % (
        slat, xlat/60, xlat%60,
        slon, xlon/60, xlon%60)

  def CSVFormat(self):
    """Format ourselves as 6 columns in a CSV file.

    The inverse of BuildFromCSV.

    Returns:
      A string with 6 comma separated values.
    """
    effective_inf = 100 * 10000
    if self.lat > effective_inf:
      xlat = effective_inf
    else:
      xlat = int(round(abs(self.lat)))

    if self.lon > effective_inf:
      xlon = effective_inf
    else:
      xlon = int(round(abs(self.lon)))

    if self.lat < 0:
      slat = -1
    else:
      slat = 1
    if self.lon < 0:
      slon = -1
    else:
      slon = 1
    return '%d,%03d,%02d, %d,%03d,%02d' % (
        slat, xlat/60, xlat%60,
        slon, xlon/60, xlon%60)

  @staticmethod
  def BuildFromCSV(row, i):
    """Given a row and an index into the row, extract 6 columns as a latlon.

    This is the inverse of CSVFormat.
    Args:
      row: the row as read from the csv.
      i: which column number (0-based) to start extracting values.

    Returns:
      A new LatLon, and the next[sic] position to start reading from.
    """
    lat = int(row[i+0]) * DM(float(row[i+1]), float(row[i+2]))
    lon = int(row[i+3]) * DM(float(row[i+4]), float(row[i+5]))
    ll = LatLon(lat, lon)
    return ll, i+6-1  # the caller assumes the whole item is length 1

  def __eq__(self, p1):
    return self.lat == p1.lat and self.lon == p1.lon

  def __hash__(self):
    hashable_tuple = (self.lat, self.lon)
    hash_code = hashable_tuple.__hash__()
    # print '%s hash==> 0x%016x' % (hashable_tuple, hash_code & 0xffffffff)
    return hash_code

  def KeyByAxis(self, axis):
    if axis == 0:
      return self.lon  # x coordinate, mostly
    elif axis == 1:
      return self.lat
    else:
      raise Exception('bad axis')

  def SquareDistance(self, p2):
    coslat = math.cos(math.radians(48.0))
    dx = (self.lon - p2.lon) * coslat
    dy = self.lat - p2.lat
    return dx * dx + dy * dy


def LatLonDeg(lat, lon):
  return LatLon(int(round(lat*60)), int(round(lon*60)))


def LatMinutesAsString(minutes):
  """Returns a short printable string for a latitude minutes value."""
  return '%03d:%02d' % (int(round(minutes / 60)), int(round(minutes % 60)))


def LonMinutesAsString(minutes):
  """Returns a short printable string for a longitude minutes value."""
  minutes_w = -minutes
  return '%01d:%02d' % (minutes_w / 60 - 120, minutes_w % 60)


def MinutesAsRadians(minutes):
  return math.radians(minutes/60.0)


def GreatCircleDistance(p1, p2):
  """Computes the great circle distance between p1 and p2.

  See http://en.wikipedia.org/wiki/Great_circle_distance
  See http://en.wikipedia.org/wiki/Great-circle_navigation

  Args:
    p1: A LatLon, the standpoint.
    p2: A LatLon, the forepoint.

  Returns:
    A pair: distance (nm), bearing (degrees).
  """

  trace = False

  if trace:
    print 'p1=%s p2=%s' % (p1, p2)
  phis = MinutesAsRadians(p1.lat)  # standpoint
  phif = MinutesAsRadians(p2.lat)  # forepoint
  lambdas = MinutesAsRadians(p1.lon)  # standpoint
  lambdaf = MinutesAsRadians(p2.lon)  # forepoint

  unused_deltaphi = phis - phif
  deltalambda = -(lambdas - lambdaf)  # positive eastward TODO(rrh): check this

  # Vincenty formula
  term_sa = math.cos(phif) * math.sin(deltalambda)
  term_sb = (math.cos(phis) * math.sin(phif)
             - math.sin(phis) * math.cos(phif) * math.cos(deltalambda))
  alphas = math.atan2(term_sa, term_sb)
  numerator = math.sqrt(term_sa*term_sa + term_sb*term_sb)
  denominator = (math.sin(phis)*math.sin(phif)
                 + math.cos(phis)*math.cos(phif)*math.cos(deltalambda))
  delta_radians = math.atan2(numerator, denominator)

  earth_radius_km = 6372.8
  nm_per_km = 0.539957
  distance_nm = delta_radians * earth_radius_km * nm_per_km

  bearing = math.degrees(alphas)
  if bearing < 0:
    bearing += 360
  return distance_nm, bearing


class Matrix(object):
  """A 2-D transformation matrix."""

  def __init__(self, diagonal_value=1.0):
    self._contents = [
        [diagonal_value, 0, 0],
        [0, diagonal_value, 0],
        [0, 0, diagonal_value]]

  def Set(self, contents):
    self._contents = contents

  def Duplicate(self):
    newrows = []
    for row in self._contents:
      newrows.append([val for val in row])
    m = Matrix()
    m._contents = newrows
    return m

  def Transform(self, point):
    pv = [point.x, point.y, 1]
    pout = []
    for r in [0, 1, 2]:
      dot_product = 0.0
      for c in [0, 1, 2]:
        dot_product += self._contents[r][c] * pv[c]
      pout.append(dot_product)
    return Point(pout[0], pout[1])

  def Rotate(self, theta):
    cos_theta = math.cos(theta)
    sin_theta = math.sin(theta)
    rmatrix = Matrix()
    # Watchout: This is for counter clock wise rotation
    rmatrix.Set([
        [cos_theta, -sin_theta, 0.0],
        [sin_theta, cos_theta, 0.0],
        [0.0, 0.0, 1.0]])
    self._Multiply(rmatrix)

  def Scale(self, scalex, scaley):
    rmatrix = Matrix()
    rmatrix.Set([
        [scalex, 0.0, 0.0],
        [0.0, scaley, 0.0],
        [0.0, 0000.0, 1.0]])
    self._Multiply(rmatrix)

  def Translate(self, translate_point):
    rmatrix = Matrix()
    rmatrix.Set([
        [1.0, 0.0, translate_point.x],
        [0.0, 1.0, translate_point.y],
        [0.0, 0.0, 1.0]])
    self._Multiply(rmatrix)

  def _Multiply(self, rhs):
    newmat = []
    for r in [0, 1, 2]:
      newrow = []
      for c in [0, 1, 2]:
        dot_product = 0.0
        for k in [0, 1, 2]:
          dot_product += self.Get(r, k) * rhs.Get(k, c)
        newrow.append(dot_product)
      newmat.append(newrow)
    self._contents = newmat

  def Get(self, i, j):
    return self._contents[i][j]

  def Print(self, output_file=sys.stdout):
    for r in [0, 1, 2]:
      for c in [0, 1, 2]:
        print >>output_file, ' %8.2f' % self._contents[r][c],
      print >>output_file, ''


class TransformStack(object):
  """A stack of Matrix, to be manipulated as in postscript."""

  def __init__(self):
    self._stack = []
    self._stack.append(Matrix())

  def Pop(self):
    self._stack.pop()

  def Push(self):
    self.Duplicate()

  def Duplicate(self):
    self._stack.append(self.Top().Duplicate())

  def Top(self):
    return self._stack[len(self._stack)-1]

  def Transform(self, point):
    return self.Top().Transform(point)

  def Translate(self, point):
    return self.Top().Translate(point)

  def Scale(self, scalex, scaley):
    return self.Top().Scale(scalex, scaley)

  def Rotate(self, theta):
    return self.Top().Rotate(theta)


class BitMatrix(object):
  """Inefficiently models a 2D bit matrix."""

  def __init__(self, name=''):
    self._data = {}
    self._name = name

  def SetPixel(self, point, value=True, trace=False):
    """Set the pixel behind point."""
    row = self._data.setdefault(int(point.y), {})
    row[int(point.x)] = value
    if trace:
      print '%s Point at %s' % (self._name, point,)

  def GetPixel(self, point):
    """Get the pixel behind point."""
    row = self._data.setdefault(int(point.y), {})
    return row.get(int(point.x), False)

  def DrawAscii(self, output_file=sys.stdout):
    """Draw the bitmap in ASCII."""
    maxx = 25
    minx = -25
    maxy = 25
    miny = -25
    for ymirror in xrange(maxy - miny):
      y = maxy - ymirror - 1
      row = ''
      for xcount in xrange(maxx - minx):
        x = xcount + minx
        is_set = self.GetPixel(Point(x, y))
        if is_set:
          ch = '*'
        else:
          ch = ' '
        row += ch
      print >>output_file, '%3d %s' % (y, row)


def BresenhamLineFloats(m, p0, p1, color):
  """Draw a line from p0_start to p1_start, marking visited pixels in m.

  Args:
    m: A PointSet.
    p0: Starting point, which uses floats.
    p1: Ending point, which uses floats.
    color: The color to use for the line.
  """
  BresenhamLine(m,
                Point(int(round(p0.x)), int(round(p0.y))),
                Point(int(round(p1.x)), int(round(p1.y))),
                color)


def BresenhamLine(m, p0_start, p1_start, color):
  """Draw a line from p0_start to p1_start, marking visited pixels in m.

  Args:
    m: A PointSet.
    p0_start: Starting point, integer screen coordinates
    p1_start: Ending point, integer screen coordinates
    color: The color to use for the line.
  """

  p0 = Point(p0_start.x, p0_start.y)  # TODO(rrh): Add/use a Point.copy member
  p1 = Point(p1_start.x, p1_start.y)  #

  dx = abs(p1.x - p0.x)
  dy = abs(p1.y - p0.y)

  if p0.x < p1.x:
    sx = 1
  else:
    sx = -1

  if p0.y < p1.y:
    sy = 1
  else:
    sy = -1
  err = dx - dy

  while True:
    m.SetPixel(p0, color)
    if p0.x == p1.x and p0.y == p1.y:
      break
    e2 = 2*err
    if e2 > -dy:
      err -= dy
      p0.x += sx
    if e2 < dx:
      err += dx
      p0.y += sy


class PointSet(object):
  """A set of points."""

  def __init__(self):
    self._set = set()

  def SetPixel(self, point, value):
    copy_point = Point(point.x, point.y)
    if value:
      self._set.add(copy_point)
    else:
      self._set.discard(copy_point)

  def GetSet(self):
    return self._set

  def Print(self):
    print 'PointSet contains:'
    for point in self._set:
      print '  %s' % (point,)


# From http://www.java-gaming.org/index.php?topic=22590.0
# converted to python
def LinesIntersect(p1, p2, p3, p4):
  return LinesIntersectWorker(
      p1.x, p1.y,
      p2.x, p2.y,
      p3.x, p3.y,
      p4.x, p4.y)


def LatLonIntersect(p1, p2, p3, p4):
  return LinesIntersectWorker(
      p1.lon, p1.lat,
      p2.lon, p2.lat,
      p3.lon, p3.lat,
      p4.lon, p4.lat)


def LinesIntersectWorker(x1, y1, x2, y2, x3, y3, x4, y4):
  """Return a boolean indicating if the two lines intersect."""
  # Return false if either of the lines have zero length
  if (x1 == x2 and y1 == y2) or (x3 == x4 and y3 == y4):
    return False

  # Fastest method, based on Franklin Antonio's
  # "Faster Line Segment Intersection" topic "
  # in Graphics Gems III" book (http://www.graphicsgems.org/)
  ax = x2-x1
  ay = y2-y1
  bx = x3-x4
  by = y3-y4
  cx = x1-x3
  cy = y1-y3

  alpha_numerator = by*cx - bx*cy
  common_denominator = ay*bx - ax*by
  if common_denominator > 0:
    if alpha_numerator < 0 or alpha_numerator > common_denominator:
      return False
  elif common_denominator < 0:
    if alpha_numerator > 0 or alpha_numerator < common_denominator:
      return False

  beta_numerator = ax*cy - ay*cx
  if common_denominator > 0:
    if (beta_numerator < 0) or (beta_numerator > common_denominator):
      return False
  elif common_denominator < 0:
    if (beta_numerator > 0) or (beta_numerator < common_denominator):
      return False

  if common_denominator == 0:
    # This code wasn't in Franklin Antonio's method.
    # It was added by Keith Woodward.
    # The lines are parallel.
    # Check if they're collinear.
    y3_less_y1 = y3 - y1

    # see http://mathworld.wolfram.com/Collinear.html
    collinearity_test_for_p3 = x1*(y2-y3) + x2*(y3_less_y1) + x3*(y1-y2)

    # If p3 is collinear with p1 and p2 then p4 will also be collinear,
    # since p1-p2 is parallel with p3-p4
    if collinearity_test_for_p3 == 0:
      # The lines are collinear. Now check if they overlap.
      if (x1 >= x3 and x1 <= x4 or x1 <= x3 and x1 >= x4 or
          x2 >= x3 and x2 <= x4 or x2 <= x3 and x2 >= x4 or
          x3 >= x1 and x3 <= x2 or x3 <= x1 and x3 >= x2):
        if (y1 >= y3 and y1 <= y4 or y1 <= y3 and y1 >= y4 or
            y2 >= y3 and y2 <= y4 or y2 <= y3 and y2 >= y4 or
            y3 >= y1 and y3 <= y2 or y3 <= y1 and y3 >= y2):
          return True

    return False

  return True
